#include "rclcpp/rclcpp.hpp"
#include "control/WMJRobotControl.h"
#include "base_interfaces/msg/navigation.hpp"

// #define DEBUG
namespace wmj
{
    void Control::publishRobotStateMassage()
    {
        if (RobotControl)
        {
            GameStateMsg game_state_msg = RobotControl->getGameState();
            auto msg = base_interfaces::msg::Game();
            game_state_msg = RobotControl->getGameState();
            // std::cout << "[game state;] " << game_state_msg.game_state << std::endl;
#ifdef DEBUG
            static bool game_start = false;
            static double time = -1;
            if (game_state_msg.game_start & 0x01 || game_start == true)
            {
                game_start = true;
                game_state_msg.game_start &= 0x01;
                if (time < 0)
                    time = wmj::now();
            }
            game_state_msg.outpost_blood = 1200;
            game_state_msg.sentry_blood = 600;
            game_state_msg.bullet_remain = 175;
            double remain_time = 350 - (wmj::now() - time);
            game_state_msg.remain_time = remain_time > 20 ? remain_time : 20;
            game_state_msg.own_alive = 2;
            game_state_msg.enemy_alive = 2;
            game_state_msg.key.X = 1;
            game_state_msg.hityaw = 0;
#endif // DEBUG
            msg.game_stage = game_state_msg.game_state;
            msg.self_hp = game_state_msg.current_hp;
            // std::cout << "hp: " <<std::hex << game_state_msg.current_hp << std::endl;
            // msg.self_max_hp = game_state_msg.max_hp;
            msg.bullet_num = game_state_msg.bullet_remain;
            msg.coin_num = game_state_msg.coin_remain;
            msg.time_left = game_state_msg.remain_time;
            msg.on_heal = game_state_msg.recovery_buff;
            // msg.self_check = game_state_msg.self_check;
            msg.game_timestamp = wmj::now();
            std::vector<int> enemy_alive;
            // std::cout << "ememy alive:"<< std::hex << game_state_msg.enemy_alive << std::endl;
            for(int i =0; i < 8 ; i++)
            {
                int data;
                data = (game_state_msg.enemy_alive >> i) & 0x01;
                enemy_alive.push_back(data);
                // game_state_msg.enemy_alive >> 1;
            }
            msg.set__enemy_alive(enemy_alive);

            msg.self_outpost_hp = game_state_msg.outpost_blood;
            msg.self_base_hp = game_state_msg.base_blood;
            msg.defend_buff = game_state_msg.defence_buff;
            
            if (game_state_msg.enemy_outpost_status == 1)
            {
                msg.enemy_outpost_hp = 1500;
            }
            else
            {
                msg.enemy_outpost_hp = 0;
            }

            // 添加底盘能量和电容能量赋值
            msg.chassis_energy_remain = game_state_msg.chassis_energy_remain;
            msg.cap_energy = game_state_msg.cap_energy;

            static int downcount = 5;
            static uint8_t key_remain = 0;
            static uint16_t x_pos_remain = 0, y_pos_remain = 0;
            if(key_remain == game_state_msg.client_key && x_pos_remain == game_state_msg.goal_position_x
            && y_pos_remain == game_state_msg.goal_position_y)
            {
                if(--downcount == 0)
                {
                    downcount = 5;
                    msg.manual_cmd_key = 0;
                    msg.goal_position_x = 0;
                    msg.goal_position_y = 0;
                }
            }
            else
            {
                downcount = 5;
                msg.manual_cmd_key = game_state_msg.client_key;
                msg.goal_position_x = game_state_msg.goal_position_x;
                msg.goal_position_y = game_state_msg.goal_position_y;
                key_remain = game_state_msg.client_key;
                x_pos_remain = game_state_msg.goal_position_x;
                y_pos_remain = game_state_msg.goal_position_y;
            }
            
            // msg.m_alive = 2;     // game_state_msg.own_alive;
            // msg.enemy_alive = 2; // game_state_msg.enemy_alive;
            //先默认为true，
            // msg.goal_position_x = game_state_msg.goal_position_x;
            // msg.goal_position_y = game_state_msg.goal_position_y;
            // msg.goal_position_z = game_state_msg.goal_position_z;
            // msg.hityaw = game_state_msg.hityaw;

            // RCLCPP_DEBUG(this->get_logger(), "outpost_blood: %d, sentry_blood: %d,\nbullet_remain: %d, remain_time: %d",
            //              game_state_msg.outpost_blood, game_state_msg.sentry_blood, game_state_msg.bullet_remain * 3, game_state_msg.remain_time);

             if (m_gameStatePublisher)
                 m_gameStatePublisher->publish(msg);
        }
    }

    void Control::publishNavigationGoal(int goal_num)
    {
        geometry_msgs::msg::PoseStamped goal_pose;
        switch (goal_num)
        {
        case 1: // A点
        {
            goal_pose.header.frame_id = "map";
            goal_pose.header.stamp = this->now();
            goal_pose.pose.position.x = 3.5359530448913574;
            goal_pose.pose.position.y = 1.6006977558135986;
            goal_pose.pose.position.z = 0.0;
            goal_pose.pose.orientation.x = 0.0;
            goal_pose.pose.orientation.y = 0.0;
            goal_pose.pose.orientation.z = 0.36713632223718634;
            goal_pose.pose.orientation.w = 0.9301671467506003;
            break;
        }
        case 2: // B点
        {
            goal_pose.header.frame_id = "map";
            goal_pose.header.stamp = this->now();
            goal_pose.pose.position.x = 3.307368516921997;
            goal_pose.pose.position.y = -1.9256173372268677;
            goal_pose.pose.position.z = 0.0;
            goal_pose.pose.orientation.x = 0.0;
            goal_pose.pose.orientation.y = 0.0;
            goal_pose.pose.orientation.z = -0.6937478635635231;
            goal_pose.pose.orientation.w = 0.7202179543728741;
            break;
        }
        default: //返回点
        {
            goal_pose.header.frame_id = "map";
            goal_pose.header.stamp = this->now();
            goal_pose.pose.position.x = 0.0;
            goal_pose.pose.position.y = 0.0;
            goal_pose.pose.position.z = 0.0;
            goal_pose.pose.orientation.x = 0.0;
            goal_pose.pose.orientation.y = 0.0;
            goal_pose.pose.orientation.z = 0.0;
            goal_pose.pose.orientation.w = 1.0;
            break;
        }
        }
        m_navigationGoalPublisher->publish(goal_pose);
    }

    void Control::publishNavigationGoalCallback()
    {
        GameStateMsg game_state_msg = RobotControl->getGameState();
        KeyMsg key = game_state_msg.key;
        if (key.CTRL && key.Q)
        {
            publishNavigationGoal(1);
        }
        else if (key.CTRL && key.E)
        {
            publishNavigationGoal(2);
        }
        else
        {
        }
    }

    void Control::keySubscription(const std_msgs::msg::Char::SharedPtr msg)
    {
        if (msg->data == 'q')
        {
            publishNavigationGoal(1);
        }
        if (msg->data == 'e')
        {
            publishNavigationGoal(2);
        }
        if (msg->data == 'D')
        {
            m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::DEFAULT;
        }
        if (msg->data == 'E')
        {
            m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::AIMER;
        }
        if (msg->data == 'N')
        {
            m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::NAVIGATION;
        }
        if (msg->data == 'A')
        {
            m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::SCAN;
        }
    }
}
